#include <pcl_utils.h>

PCL_UTILS::PCL_UTILS():
        viewer(new pcl::visualization::PCLVisualizer("viewer")),
        clicked_points_3d(new pcl::PointCloud<PointCloudType>),
        pc(new PointCloud<PointCloudType>){
        num = 0;
        pcd_file_num = 0;
}

PCL_UTILS:: ~PCL_UTILS(){
}

/**
 * @brief 读取pcd文件，填充pointcloud成员
 * @param  filename         读取的pcd文件的文件名
 * @return true         读取成功
 * @return false        读取失败
 */
bool PCL_UTILS::read_pcd_file(const char * filename){
        if(io::loadPCDFile(filename, *pc) == -1){  //读取pcd文件到点云指针中
                cerr << "can't read pcd file!" << endl;
                return false;
        }
}

void PCL_UTILS::write_pcd(pcl::PointCloud<PointCloudType>::Ptr pcd_cloud){
        std::ofstream out;
	std::string file = std::string(PCD_FOLDER);
	file.append(PCD_PREFIX).append(std::to_string(pcd_file_num++)).append(PCD_SUBFIX);
        out.open(file.c_str());
        out << PCD_HEAD << pcd_cloud->points.size() <<
                        PCD_MID << pcd_cloud->points.size()<< PCD_RAIL << std::endl;
        for (long i = 0; i < pcd_cloud->points.size() ; i ++){
                out << pcd_cloud->points[i].x << ' ' << pcd_cloud->points[i].y << ' ' << 
                        pcd_cloud->points[i].z << ' ' << pcd_cloud->points[i].intensity << std::endl;
        }
	out.close();
}

void PCL_UTILS::bbox2pcd_file(PointCloud<PointCloudType>::Ptr point_cloud,float x, float y, float z, float length, float width, float height, float angle){
        PointCloud<PointCloudType>::Ptr new_point_cloud(new PointCloud<PointCloudType>);
        //将点云逆时针旋转
        for(auto i = point_cloud->points.begin(); i < point_cloud->points.end(); i++){
                PointCloudType single_point;
                float new_x = cos(-angle)* (i->x-x) + (-1)*sin(-angle) * (i->y-y)+x;
                float new_y = sin(-angle)* (i->x-x) + cos(-angle) * (i->y-y)+y;

                if(new_x > x-length/2 && new_x < x+length/2 && 
                        new_y > y-width/2 && new_y < y+width/2 && 
                                i->z > z-height/2 && i->z < z+height/2){
                // if(new_x > x-length/2-1 && new_x < x+length/2+1 && 
                //         new_y > y-width/2-1 && new_y < y+width/2+1 && 
                //                 i->z > z-height/2-1 && i->z < z+height/2+1){
                                        single_point.x = i->x;
                                        single_point.y = i->y;
                                        single_point.z = i->z;
                                        single_point.intensity = i->intensity;
                                        new_point_cloud->points.push_back(single_point);
                                }
                else{
                        continue;
                }
        }
        write_pcd(new_point_cloud);
}


void PCL_UTILS::display_pcd_files(const char * filename, bool area_picking = false){

        if(io::loadPCDFile(filename, *pc) == -1){  //读取pcd文件到点云指针中
                cerr << "can't read pcd file!" << endl;
                return;
        }
	pcl::visualization::PointCloudColorHandlerGenericField<PointCloudType> fildColor(pc, "intensity"); //根据点云强度涂色
        viewer->addPointCloud<PointCloudType>(pc,fildColor ,"sample_cloud"); //我们将点云添加到视窗对象中，并定义一个唯一的字符串作为ID号，
                //利用此字符串保证在其他成员方法中也能标识引用该点云，多次调用addPointCloud()，可以实现多个点云的添加，
                //每调用一次就创建一个新的ID号，如果想更新一个已经显示的点云，用户必须先调用removePointCloud()，并提供需要更新的点云的ID号。
                //（注：PCL 的1.1及以上版本提供一个新的API，updatePointCloud()，通过该接口，不必手动调用removePointCloud()，就可实现点云的更新）

        viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample_cloud");
        if(area_picking){
                viewer->registerAreaPickingCallback(&PCL_UTILS::AreaPicking_callback, *this);
        }
        //通过设置照相机参数使得从默认的角度和方向观察点云
        viewer->setCameraPosition(0, 0, -2, 0, -1, 0, 0);
        viewer->spin();
}

bool PCL_UTILS::filter_pc(float min_x, float max_x, float min_y, float max_y, float min_z, float max_z){
        pcl::PassThrough<PointCloudType> pass;  //创建滤波器对象
        pass.setInputCloud(pc);
        pass.setFilterFieldName ("x");             //设置在X轴方向上进行滤波
        pass.setFilterLimits (min_x, max_x); 
        pass.setFilterFieldName ("y");             //设置在Y轴方向上进行滤波
        pass.setFilterLimits (min_y, max_y); 
        pass.setFilterFieldName ("z");             //设置在Z轴方向上进行滤波
        pass.setFilterLimits (min_z, max_z); 
        pass.filter(*pc);
}


void PCL_UTILS::AreaPicking_callback(const pcl::visualization::AreaPickingEvent& event, void* args){
        std::vector< int > indices;
        if (event.getPointsIndices(indices)==-1)
                return;

        for (int i = 0; i < indices.size(); ++i)
        {
                clicked_points_3d->points.push_back(pc->points.at(indices[i]));
        }
        write_pcd(clicked_points_3d);

        pcl::visualization::PointCloudColorHandlerCustom<PointCloudType> red(clicked_points_3d, 255, 0, 0);

        std::stringstream ss;
        std::string cloudName;
        ss << num++;
        ss >> cloudName;
        cloudName += "_cloudName";

        viewer->addPointCloud(clicked_points_3d, red, cloudName);
        viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10, cloudName);
}



